/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* CScanMatchInterface
* An C++ interface for Scan matching based pose estimation.
* The interface specifies the interface functions that your module must implement.
* All the units must be in SI. 
* The coordinates that are given are always cartesian coordinates. Also, note that the scan pose (meaning
* the pose the scan is estimated to be taken from) is always in global coordinates!
* 
*
* @author Jari
*
*/
#ifndef _C_SCAN_MATCH_INTERFACE_H_
#define _C_SCAN_MATCH_INTERFACE_H_
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

/**
*NOTE: These data types are now specified here but they SHOULD be defined in LnMlib Datatypes.
*TODO: Well Fix it!
*/
namespace ScanMatch{
    /**
    * Definition of scan for Scan Match interface
    * Uses the polar coordinate format (r,a) pairs. 
    * Natural for sensor coordinates and used as sensor coordinates here!
    */
	struct scan{
	    public:
		float *r;  ///< Distance measurement vector [m]
		float *a;  ///< Angle measurement of distance measurement [rad] 
		int N;     ///< The number of measurements in vector
		scan(){
		    N=0;
		    r=NULL;
		    a=NULL;
		}
		~scan(){
		    if(r) free(r);
		    if(a) free(a);
		}
                void allocate(int n){
									if(n<=0){
										fprintf(stderr,"Scanmatch::scan::allocate:: INVALID SCAN SIZE, --- %d \n",n);
										return;
									}
									if(r) free(r);
									if(a) free(a);	
                  r = (float *) malloc(n*sizeof(float));
                  a = (float *) malloc(n*sizeof(float));
									N=n;
                }
		void copy(struct scan &new_scan){
				if(N<=0){
						fprintf(stderr,"Scanmatch::scan::copy:: INVALID SCAN SIZE=%d\n",N);
						return;
				}
				if(!new_scan.r) new_scan.r = (float *) malloc(N*sizeof(float));
				if(!new_scan.a) new_scan.a = (float *) malloc(N*sizeof(float));
		    new_scan.N = N;
        memcpy(new_scan.r,r,N*sizeof(float));
        memcpy(new_scan.a,a,N*sizeof(float));
		}
		void filt(float d){
		  for(int i=0;i<N;i++){
			 if(r[i] > d) r[i] = 0;
			}	
		}
		
	};
	/**
	* ScanMatch pose definition
	* 2D-pose x,y and heading 
	*/
	struct pose{
	    public:
		float x; ///<[m]
		float y; ///<[m]
		float a; ///<[rad]
		pose(){
		    x=0;
		    y=0;
		    a=0;
		}
		pose(float _x, float _y,float _a){
				a= _a;
				x=_x;
				y=_y;
		}
		void set(pose p){
		    x=p.x;
		    y=p.y;
		    a=p.a;
		}
   void set(float xx,float yy, float aa){
		x=xx;
		y=yy;
		a=aa;
   }
		~pose(){}
		
		/**
		* Calculates the differential movement of the sensor between two odometric position
		* (reference scan and current measurement)
		* This is used as initial estimate for the scan matching algorithms
		* @param odo_ref The odometric position of the reference scan
		* @param odo_cur The odometric position of the most recent scan
		* @return pose The differential movement with respect to global frame
		*/
		const pose calculateDifferentialSensorMovement(const pose odo_ref,const pose odo_cur, float sensor_offset){
		    	float ddx,ddy,dist,alpha;
			pose out,tmp;
			///Calculates the differential movement in odometry frame of reference
			ddx = odo_cur.x - odo_ref.x;
			ddy = odo_cur.y - odo_ref.y;
			alpha = atan2(ddy, ddx);
			dist = sqrt(ddx*ddx+ddy*ddy);
	
			tmp.x = dist * cos( alpha - odo_ref.a );
			tmp.y = dist * sin( alpha - odo_ref.a );
			tmp.a = (float)fmod((float)(odo_cur.a - odo_ref.a), (float)(2.0*M_PI)); 
			if(tmp.a < 0) tmp.a += 2*(float)M_PI;
			
			///Rotate to the sensor coordinates 
			out.x = cos(sensor_offset)*tmp.x - sin(sensor_offset)*tmp.y;
			out.y = sin(sensor_offset)*tmp.x + cos(sensor_offset)*tmp.y;
			out.a = tmp.a;
			//out.set(tmp);
			return out;
		}
		/**
		* Integrates new position from this position and differential position 
		* @param diff the differential position to be added to this position
		* @return pose New integrated position
		**/
		const pose integrateDifferential(const pose diff){
		    pose result;
		    float l,phii;
		    
				l = sqrt(diff.x*diff.x + diff.y*diff.y);
				phii = atan2(diff.y,diff.x);
				
		    result.x = x + (float)cos(a+phii)*l;
		    result.y = y + (float)sin(a+phii)*l;

		    // Update angle
		    result.a = (float)fmod((float)(diff.a + a), (float)(2.0*M_PI));
		    // fmod accepts also negative values
		    if(result.a < 0) result.a += 2*(float)M_PI;
		    return result; 
		}
		void topi(){
			if(a>M_PI) while(a>M_PI) a-=2.0*M_PI;
			else if(a<-M_PI) while(a<M_PI) a+=2.0*M_PI;
		}
	};
}


class CScanMatchInterface{
    private:
    protected:
	/**
	* ScanMatch::pose pos is the position after the scan matching is done
	* Also known as the current pose. In other words the result of the update-function
	* is integrated to ref pose and the result is this pose (ok?)
	*/
	ScanMatch::pose pos;    
	ScanMatch::pose rpos;  ///< This is the position of the reference scan
  ScanMatch::scan ref;   ///< The Reference scan 
        	
    public:
	/**
	* Default Constructor
	* Initialize your module with parameters in your own constructor.
	*/
	CScanMatchInterface(){}
	/**
	* destructor
	*/
	virtual ~CScanMatchInterface(){}
	
	/**
	* Read the current position estimate
	* @param &cur reference to ScanMatch::pose  
	*/
	void getCurrentPose(ScanMatch::pose &cur){
	    cur.set(pos);
	}
	/**
	* Set the current pose. 
	*/
	void setCurrentPose(ScanMatch::pose cur){
	    pos.set(cur);
	}
	/**
	 * Set the reference pose. 
	 */
	void setReferencePose(ScanMatch::pose cur){
	    rpos.set(cur);
	}
	
	/**
	* Set the reference scan. The newest measurement should be compared against this one.
	* The position of this scan is assumed to be the latest estimated position!
	* @param &ref ScanMatch::scan, users respossibility to fill correctly... (can it be other way?)
	*/
		void setReferenceScan(ScanMatch::scan &new_ref){
				new_ref.copy(ref);
				setReferencePose(pos);
		}
	/**
	* Set the reference scan. The newest measurement should be compared against this one
        * Sets the current estimated pose to @p
	* @param &ref ScanMatch::scan, users respossibility to fill correctly... (can it be other way?)
	* @param &r ScanMatch::pose, the pose, where the scan is taken from.
	*/
		void setReferenceScan(ScanMatch::scan &new_ref, ScanMatch::pose p){
				new_ref.copy(ref);
				setReferencePose(p);
		}
		/**
		* Updates the position estimate with the new measurement @meas. 
		* @param &meas The latest range scan measurement
		* @param &est  The estimated position, where the measurement is from.
		* @return The goodness (probability/likelihood) of the match [0...1] 1 is perfect 0 is none 
		*/
		virtual float update(ScanMatch::scan &meas, ScanMatch::pose est)=0;
	
};

#endif
